package com.gitee.wsl.mathematics.matrix.mat3.ext

import com.gitee.wsl.mathematics.matrix.N
import com.gitee.wsl.mathematics.matrix.mat3.Matrix3
import com.gitee.wsl.mathematics.vector.ext.cross
import com.gitee.wsl.mathematics.vector.ext.dot
import com.gitee.wsl.mathematics.vector.vec2.Vector2
import com.gitee.wsl.mathematics.vector.vec3.Vector3

/**
 * Solve A * x = b, where b is a column vector. This is more efficient than computing the inverse
 * in one-shot cases.
 *
 * @param b
 * @return
 */
fun <T:Number> Matrix3<T, *, *>.solve22ToOut(b: Vector2<T,*>):Vector2<T,*> {
    val a11 = m00
    val a12 = m01
    val a21 = m10
    val a22 = m11
    var det = a11 * a22 - a12 * a21
    if (det != 0.0f) {
        det = 1.0f / det
    }
    val x = det * (a22 * b.x - a12 * b.y)
    val y = det * (a11 * b.y - a21 * b.x)
    return b.create(x,y) as Vector2<T,*>
}

/**
 * Solve A * x = b, where b is a column vector. This is more efficient than computing the inverse
 * in one-shot cases.
 *
 * @param b
 * @param out the result
 */
fun <T:Number> Matrix3<T, *, *>.solve33ToOut(b: Vector3<T, *>):Vector3<T,*> {
    val ex = N[0]
    val ey = N[1]
    val ez = N[2]
    var out :Vector3<T, *> = (ey cross ez) as Vector3<T, *>
    var det = ex dot out
    if (det != 0.0f) {
        det = 1.0f / det
    }
    //Vec3.crossToOutUnsafe(ey, ez, out)
    //val x = det * Vec3.dot(b, out)
    //Vec3.crossToOutUnsafe(b, ez, out)
    //val y = det * Vec3.dot(ex, out)
    //Vec3.crossToOutUnsafe(ey, b, out)
    //val z = det * Vec3.dot(ex, out)
    //out.x = x
    //out.y = y
    //out.z = z

    out = (ey cross ez) as Vector3<T, *>
    val x = det * (b dot out)
    out = (b cross ez) as Vector3<T, *>
    val y = det * (ex dot out)
    out = (ey cross b) as Vector3<T, *>
    val z = det * (ex dot out)
    return b.create(x,y,z) as Vector3<T, *>
}
